#include "sizeclassifiersvc.h"

SizeClassifierSVC::SizeClassifierSVC(){
    this->buffer_length = BUFFER_LENGTH;
    this->com_ratio = COM_RATIO;
    Init_Infan_data();
//    cout<<Infan_vectors[0]<<Infan_vectors[2228]<<endl;
}

int SizeClassifierSVC::predict(double features[]){
    int i, j, k, d, l;

    double kernels[N_VECTORS];
    double kernel;
    switch (KERNEL_TYPE) {
        case 'l':
            // <x,x'>
            for (i = 0; i < N_VECTORS; i++) {
                kernel = 0.;
                for (j = 0; j < N_FEATURES; j++) {
                    kernel += Infan_vectors[i][j] * features[j];
                }
                kernels[i] = kernel;
            }
            break;
        case 'p':
            // (y<x,x'>+r)^d
            for (i = 0; i < N_VECTORS; i++) {
                kernel = 0.;
                for (j = 0; j < N_FEATURES; j++) {
                    kernel += Infan_vectors[i][j] * features[j];
                }
                kernels[i] = pow((KERNEL_GAMMA * kernel) + KERNEL_COEF, KERNEL_DEGREE);
            }
            break;
        case 'r':
            // exp(-y|x-x'|^2)
            for (i = 0; i < N_VECTORS; i++) {
                kernel = 0.;
                for (j = 0; j < N_FEATURES; j++) {
                    kernel += pow(Infan_vectors[i][j] - features[j], 2);
                }
                kernels[i] = exp(-KERNEL_GAMMA * kernel);
            }
            break;
        case 's':
            // tanh(y<x,x'>+r)
            for (i = 0; i < N_VECTORS; i++) {
                kernel = 0.;
                for (j = 0; j < N_FEATURES; j++) {
                    kernel += Infan_vectors[i][j] * features[j];
                }
                kernels[i] = tanh((KERNEL_GAMMA * kernel) + KERNEL_COEF);
            }
            break;
    }

    int starts[N_ROWS];
    int start;
    for (i = 0; i < N_ROWS; i++) {
        if (i != 0) {
            start = 0;
            for (j = 0; j < i; j++) {
                start += Infan_weights[j];
            }
            starts[i] = start;
        } else {
            starts[0] = 0;
        }
    }

    int ends[N_ROWS];
    for (i = 0; i < N_ROWS; i++) {
        ends[i] = Infan_weights[i] + starts[i];
    }

    if (N_CLASSES == 2) {

        for (i = 0; i < N_VECTORS; i++) {
            kernels[i] = -kernels[i];
        }

        double decision = 0.;
        for (k = starts[1]; k < ends[1]; k++) {
            decision += kernels[k] * Infan_coeff[0][k];
        }
        for (k = starts[0]; k < ends[0]; k++) {
            decision += kernels[k] * Infan_coeff[0][k];
        }

        decision += Infan_intercepts[0];

        if (decision > 0) {
            return 0;
        }
        return 1;

    }

    double decisions[N_INTERCEPTS];
    double tmp;
    for (i = 0, d = 0, l = N_ROWS; i < l; i++) {
        for (j = i + 1; j < l; j++) {
            tmp = 0.;
            for (k = starts[j]; k < ends[j]; k++) {
                tmp += kernels[k] * Infan_coeff[i][k];
            }
            for (k = starts[i]; k < ends[i]; k++) {
                tmp += kernels[k] * Infan_coeff[j - 1][k];
            }
            decisions[d] = tmp + Infan_intercepts[d];
            d = d + 1;
        }
    }

    int votes[N_INTERCEPTS];
    for (i = 0, d = 0, l = N_ROWS; i < l; i++) {
        for (j = i + 1; j < l; j++) {
            votes[d] = decisions[d] > 0 ? i : j;
            d = d + 1;
        }
    }

    int amounts[N_CLASSES];
    for (i = 0, l = N_CLASSES; i < l; i++) {
        amounts[i] = 0;
    }
    for (i = 0; i < N_INTERCEPTS; i++) {
        amounts[votes[i]] += 1;
    }

    int classVal = -1;
    int classIdx = -1;
    for (i = 0; i < N_CLASSES; i++) {
        if (amounts[i] > classVal) {
            classVal = amounts[i];
            classIdx= i;
        }
    }
    return classIdx;
}
\
int SizeClassifierSVC::BufferPredict(double features[]){
    int res = predict(features);

    if (buffer.size() < buffer_length){
        buffer.push_back(res);
    }else if (buffer.size() == buffer_length){
        buffer.push_back(res);
        buffer.pop_front();
    }else if (buffer.size() > buffer_length){
        cout<<"unexpeted situation ! buffer size > buffer_length"<<endl;
    }

    int num_small = 0;
    int num_big   = 0;
    for (deque<int>::const_iterator it=buffer.begin(); it != buffer.end(); it++){
        if (*it == 0){
            num_small++;
        }else if (*it == 1){
            num_big++;
        }else{
            cout<<"get unexpected number! "<<endl;
        }
    }

    int armor_type = 2;
    num_big = (int)(com_ratio * num_big);
    if (num_small > num_big){
        armor_type = 0;
    }else if (num_small < num_big){
        armor_type = 1;
    }else if (num_small == num_big){
        armor_type = buffer.back();
    }else{
        cout<<"armor type judge fail!"<<endl;
    }

    if (armor_type == 2){
        cout<<"fuck it no!!!"<<endl;
    }

    return armor_type;
}

int SizeClassifierSVC::Init_Infan_data(){
    FILE* fp = fopen("/home/nuc/Desktop/infantry_vision/SVC_v2/Infan_vectors.txt", "r");
    if (fp == NULL)
        {
            printf("文件读取错误...");
            return -1;
        }
    for (int i = 0; i < 2229; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            fscanf(fp, "%lf", &Infan_vectors[i][j]);/*每次读取一个数，fscanf函数遇到空格或者换行结束*/
        }
        fscanf(fp, "\n");
    }
    fclose(fp);

    FILE* fp2 = fopen("/home/nuc/Desktop/infantry_vision/SVC_v2/Infan_coeff.txt", "r");
    if (fp2 == NULL)
        {
            printf("文件读取错误...");
            return -1;
        }
    for (int i = 0; i < 2229; i++)
    {
        fscanf(fp2, "%lf", &Infan_coeff[0][i]);/*每次读取一个数，fscanf函数遇到空格或者换行结束*/
    }
    fclose(fp2);

    return 0;
}


/*
return armor_type,   small/0,     big/1
*/
int SizeClassifierSVC::Infan_SizePredict(vector<Point> all_point){
    Point left_top_point     = all_point[0];
    Point left_bottom_point  = all_point[1];
    Point right_top_point    = all_point[2];
    Point right_bottom_point = all_point[3];


    double w_w = (double)(max(fabs(left_top_point.x-right_top_point.x),
                    fabs(left_bottom_point.x-right_bottom_point.x)));
    double h_h = (double)(max(fabs(left_top_point.y-left_bottom_point.y),
                    fabs(right_top_point.y-right_bottom_point.y)));
    double w_h = w_w / h_h;
    double k_k = (double)(fabs((left_top_point.y-right_top_point.y)/
                     (left_top_point.x-right_top_point.x)));

    double features[3] = {w_h, k_k, h_h};
    int res = BufferPredict(features);

    return res;
}




